Geometric based path prediction method using moving and stop objects

ABSTRACT

A method for estimating the curvature in a road is disclosed. The method measures an azimuth angle range and relative velocity between a host and a target vehicle, which determines whether a host vehicle is changing lanes or whether a target vehicle is changing lanes. The method calculates a heading angle of the host vehicle and calculates a corrected azimuth angle by adjusting the measured azimuth angle by the value of calculated heading angle. This method selects a curve that minimizes the mean square error between the curve and selected targets; and determines an equation that describes the curve, wherein the equation is used to predict the path head of the host vehicle.

TECHNICAL FIELD

[0001] The present invention relates to systems and methods forpredicting the path of a host vehicle for safety and non-safetyautomotive applications, such as adaptive cruise control (ACC).

BACKGROUND

[0002] Adaptive cruise control systems are gaining wide spreadacceptance in vehicles today. Adaptive cruise control (ACC) systemsutilize a conventional cruise control system, which maintains a desiredvehicle speed. In addition, the adaptive cruise control system canmodify the speed of the vehicle to accommodate for changes in trafficconditions. The ACC system accomplishes this through automaticacceleration, deceleration and/or braking. Thus, the vehicle having theACC system (which will be referred to herein as the host vehicle)maintains a safe distance from the vehicle driving directly in front ofthe host vehicle (this vehicle will be referred to the target vehicle)as a function of road speed.

[0003] Typically prior art adaptive cruise control systems include anadaptive cruise control processor, a radar sensor, a brake interventionsystem, a display unit, an engine intervention system, a plurality ofsensors (i.e., wheel speed, yaw rate, steering wheel angle, lateralacceleration), and a transmission intervention system. Typically, theradar sensor operates at a frequency of 76 to 77 gigahertz, which wasspecifically allocated for ACC systems. In operation, radar beam isemitted by the host vehicle and is reflected off of the target vehicleback toward the host vehicle. The propagation time, dopier shift, andamplitude of the emitted and reflected radar waves are compared. Fromthis comparison, the range or distance, relative speed and relativeposition between the target and host vehicles are calculated.

[0004] One significant problem for ACC systems to overcome is to ensurereliable operation of the system in varying situations such as curves inthe road and/or during lane changes. For proper system operation, it isessential that the target vehicle is correctly identified and the lanein which the target vehicle is located is also identified. Prior artsystems obtain information from a yaw rate sensor, a steering wheelangle sensor, wheel speed sensors, and lateral speed sensors todetermine the target vehicle's lane location and curve status. Othersystems under consideration for determining vehicle location are videoimaging systems.

[0005] Methods found in literature use the yaw rate and the vehiclespeed to calculate the curvature of the road. The shortcomings of thismethod are: first, the path or curvature of the road cannot beaccurately predicted and second, any prediction is highly affected bythe driver behavior. The first shortcoming is due to the fact that thecalculated curvature from the yaw rate and the speed measurementsrepresents the road curvature at the host vehicle position, and thesensors used have different kinds of measurements errors. The lattershortcoming is due to driver habit where he or she doesn't follow theroad curvature, e.g., during a lane change. Other prior art methods thatuse target information to estimate the curvature of the road assume thatthe host vehicle is always following the road. Therefore, these methodsfail when a host vehicle maneuvers or changes lanes.

[0006] Therefore, what is needed is a new and improved method forovercoming these shortcomings. This new and improved method shouldaccurately predict the location of the target vehicle without the needfor extensive experimental data.

SUMMARY

[0007] The method of the present invention utilizes what will bereferred to as a projected host vehicle reference frame. The projectedhost reference frame results from rotating a host vehicle referenceframe to align the host vehicle reference frame with a road referenceframe. This is achieved by determining whether the host vehicle ischanging lanes and accounting for a heading angle of the host vehiclewith respect to the road. In addition, the present invention utilizesstopped and moving objects to obtain the maximum benefit of the existingobjects in the radar field of view. Moving objects are perceived in twoways, first as moving object with history, and second as a stoppedobject at the current time. Also, the stopped objects such as aguardrail or a row of trees on a road side can perceived as a fictitiousmoving object that travels at the host vehicle speed.

[0008] In an aspect of the present invention, a method for estimatingthe curvature in a road is provided. This method uses an azimuth anglerange and relative velocity between a host and a target vehicle radarmeasurement to determine whether the host vehicle is changing lanes orwhether the target vehicle is changing lanes.

[0009] In another aspect of the present invention, the method calculatesa heading angle of the host vehicle and calculates a corrected azimuthangle by adjusting the measured azimuth angle by the value of calculatedheading angle

[0010] In a further aspect of the present invention, the method selectsa curve that minimizes the mean square error between the curve andselected targets, and determines an equation that describes the curve,wherein the equation is used to predict the path ahead of the hostvehicle.

[0011] These and other aspects and advantages of the present inventionwill become apparent upon reading the following detailed description ofthe invention in combination with the accompanying drawings.

BRIEF DESCRIPTION OF THE FIGURES

[0012]FIG. 1 illustrates a host vehicle having an adaptive cruisecontrol (ACC) system, in accordance with the present invention;

[0013]FIG. 2 illustrates a host vehicle following or tracking a targetvehicle, in accordance with the present invention;

[0014]FIG. 3 is a schematic diagram of a host vehicle traveling along aroad following target vehicles at a time “κ” and “κ+1”, in accordancewith the present invention;

[0015]FIG. 4 illustrates a host vehicle tracking a group of targetsalong a path preceding the host vehicle; and

[0016]FIG. 5 is a flowchart illustrating the method for determining thecurvature of a road.

DETAILED DESCRIPTION

[0017] Referring now to FIG. 1, a host vehicle 10 is illustrated havingan adaptive cruise control (ACC) system 12, in accordance with thepresent invention. Adaptive cruise control system 12 includes aplurality of vehicle sensors for measuring various vehicle dynamicsparameters. For example, ACC system 12 includes a yaw rate sensor 14 formeasuring the yaw rate of host vehicle 10. Other vehicle sensors includea speed sensor 16, for measuring vehicle speed, and a range sensor 18for detecting objects and other vehicles (target vehicles) in front ofhost vehicle 10. Further ACC system 12 includes, a control module 20mounted within host vehicle 10 and in communication with the varioussensors, just described, as well as vehicle subsystems such as thevehicle braking system 24 and the vehicle acceleration system (notshown). Preferably a controlled area network (CAN) bus 26 interconnectsthe various sensors and vehicle subsystems to control module 20.

[0018]FIG. 2 is a diagram depicting host vehicle 10 following ortracking a target vehicle 30. Range sensor 18, preferably is a radarsensor that provides relative speed, azimuth angle and distanceinformation of target vehicle 30 or a plurality of vehicles or otherobjects in the path of host vehicle 10. A fixed radar beam 32 having afrequency of 76 GHz is transmitted from radar sensor 18 for detectingmoving objects such as target vehicle 30, as well as stopped objectssuch as guardrail 34.

[0019] In operation, ACC system 12 automatically adjusts the hostvehicle's speed and then returns host vehicle 10 to the set or desiredspeed after the traffic clears. The ACC system 12 in order to operateproperly must determine, out of all of the vehicles and objects in frontof the host, which vehicle is the primary target. In order to identifythe primary in-lane target vehicle, a reliable estimation of roadcurvature ahead of the vehicle must be determined.

[0020] Referring now to FIG. 3, a diagrammatic representation of hostvehicle 10 equipped with an ACC system 12 is illustrated traveling alonga road 50, wherein road 50 has a left lane 52 and a right lane 54. Hostvehicle 10 is further shown following a target vehicle 30. Further, road50 is curved and has a radius of curvature “r” about a center point 56.As host vehicle 10 travels on road 52, radar sensor 18 measures therange, azimuth angle and relative velocity between host vehicle 10 andtarget vehicle 30 which is in the same left lane 52 of road 50 as thehost vehicle. These radar measurements occur at a time “κ” and at a time“κ+1”. At time “κ+1” host vehicle 10 has moved into right lane 54 ofroad 50 and as illustrated target vehicle 30 also has also moved intolane 54.

[0021]FIG. 3 further illustrates azimuth angle “α” and heading angle“η”. Heading angle “η” is a result of the directional motion of hostvehicle 10 from a first position “p(κ)” to a position “p(κ+1)” on road50. The azimuth angle “α” is a function of geometry and orientation ofhost vehicle 10 with respect to the road 50. The geometry factor is aresult of the relative lateral distance between host vehicle 10 andtarget vehicle 30, as well as the range between them. The orientation ofhost vehicle 10 with respect to the road is effected by the maneuveringof host vehicle 10 with respect to road 50. For example, in thesituation where the relative velocity between the host and the targetvehicles is zero, the azimuth angle varies as the host vehicle isrotating around its axis even though the geometry is not changing. Inthe non-zero relative speed situation, any maneuvering of host vehicle10 with respect to the road 50 affects the azimuth angle by both thegeometry factor and the orientation factor. On the other hand, themaneuvering of the target vehicle with respect to the road effects theazimuth angle measurement by the geometry factor only. Therefore, thegeometry factor variation is a combined result of the host and targetvehicle maneuvering.

[0022] In an aspect of the present invention, a method for predictingroad curvature is provided. As will be described hereinafter, thismethod accounts for host vehicle maneuvering, target vehiclemaneuvering, and changes in the curvature of the road.

[0023] The present invention assumes that the curvature of the roadremains constant between time samples “κ” and “κ+1”. Thus, the variationin the curvature of the road is neglected. The inventor of the presentinvention believes this to be a valid assumption since the roadcurvature doesn't change rapidly. Furthermore, the curvature of the roadis presumed to follow a circle.

[0024] The present invention addresses the difficult task ofdistinguishing between a host vehicle maneuvering and a lead vehiclemaneuvering. This task is not trivial since both types of vehiclemaneuvering have almost a similar effect on the azimuth anglemeasurement. One method is to look to the yaw rate measurement of thehost vehicle. This method, however, is not reliable for two reasons: thefirst by the difficulty of distinguishing the source of the yaw rate,i.e., is it a result of a curve, lane change, or just a yaw rate bias,and second by the noise and drift imposed on the yaw rate measurement.However, a method found to be very reliable, is to follow this rule:when host vehicle maneuvering occurs, all the azimuth angle measurementsof the targets in the radar field of view change in the same way, whilewhen a target vehicle maneuvers the azimuth angle measurement of thatspecific target changes.

[0025] Referring now to FIGS. 4 and 5, a method for determining a road'scurvature will now be described. As shown in FIG. 4, host vehicle 10 hasa group of targets 60, 62 and 64 located at points xi and yi along apath preceding host vehicle 10. Furthermore, host vehicle 10 has aheading angle “η” that is defined as the angle between a line tangent tothe road and a direction the host vehicle is heading in. A uniqueazimuth angle “α” is associated with each target 60, 62 and 64. Thus, anazimuth angle α′ is the angle between the road's tangent line and a linedrawn between the host vehicle 10 and target 60. Similarly, azimuthangle α″ is the angle between the road's tangent line and the line drawnbetween host vehicle 10 and target 62. Finally, the azimuth angle α′″ isdefined as the angle between the road's tangent line and a line drawnbetween host vehicle 10 and target 64.

[0026] With specific reference to FIG. 5, the method for determining thecurvature of a road is further illustrated. The method is initiated atblock 70, and at block 72, the ACC system of the host vehicle measuresthe range, azimuth angle, and relative velocity between the host vehicle10 and each target. It is first determined whether the target object,target vehicle or vehicles are maneuvering, at block 74. At block 76, itis determined whether the host vehicle is maneuvering. The methoddetermines whether the target or host vehicles are maneuvering byfollowing predefined rule: (1) the host vehicle is maneuvering, when allof the azimuth angle measurements of all of the targets change in thesame way; or (2) that the target vehicle is maneuvering, when only theazimuth angle of that target is changing and the azimuth angles of theother targets are not changing. Once it is determined whether the hostvehicle is changing lanes or whether a target vehicle is changing lanes,the heading angle of the host vehicle with respect to the road may nowbe estimated. From the geometry of the road and dynamics of the hostvehicle, the heading angle “η” can be estimated by solving the followingdifferential equation, as represented by block 80: $\begin{matrix}{{{{\frac{V_{h}}{L}x\quad \alpha} + \alpha} = {{{- \eta}\quad {where}\text{:}V_{h}} = {{the}\quad {host}\quad {vechicle}\quad {speed}}}};} \\{{L = {{the}\quad {range}\quad {between}\quad {the}\quad {host}\quad {vehicle}\quad {and}\quad {the}\quad {target}\quad {vehicle}}};} \\{\alpha = {{the}\quad {azimuth}\quad {angle}\quad {between}\quad {the}\quad {host}\quad {vehicle}\quad {and}\quad {the}\quad {target}\quad {{vehicle}.}}}\end{matrix}$

[0027] L=the range between the host vehicle and the target vehicle;

[0028] α=the azimuth angle between the host vehicle and the targetvehicle.

[0029] The targets coordinates of each target are mapped, as representedby block 82. Further, the method corrects the mapped target coordinatesby adjusting (rotating) the mapped target coordinates by the calculatedheading angle, as represented by block 84. At block 86, the curvature ofthe road is calculated fitting a curve through the mapped targetcoordinates x_(i) and y_(i). An optimal curve is selected that minimizesthe mean square error of the differences between the curve and eachtarget location. The curve is constrained to be a circle. Thus, thefollowing equation may be used to calculate the center of an optimalcircle through the targets: $\begin{matrix}{X_{c} = \frac{\left( {{\sum\limits_{i = 1}^{N}x_{i}^{3}} + {\sum\limits_{i = 1}^{N}{x_{i}y_{i}^{2}}} - {\frac{\sum\limits_{i = 1}^{N}{x_{i}y_{i}}}{\sum\limits_{i = 1}^{N}y_{i}^{2}}\left( {{\sum\limits_{i - 1}^{N}y_{i}^{3}} + {\sum\limits_{i = 1}^{N}{x_{i}^{2}y_{i}}}} \right)}} \right)}{2\left( {{\sum\limits_{i = 1}^{N}x_{i}^{2}} - \frac{\left( {\sum\limits_{i = 1}^{N}{x_{i}y_{i}}} \right)^{2}}{\sum\limits_{i = 1}^{N}y_{i}^{2}}} \right)}} \\{y_{c} = \frac{\left( {{\sum\limits_{i = 1}^{N}y_{i}^{2}} + {\sum\limits_{i = 1}^{N}{x_{i}^{2}y_{i}}} - {2x_{c}{\sum\limits_{i = 1}^{N}{x_{i}y_{i}}}}} \right)}{2\quad {\sum\limits_{i = 1}^{N}y_{i}^{2}}}}\end{matrix}$

[0030] The radius of curvature of the optimal circle through the targetsmay be calculated using the equation: (x_(i)−x_(c))²+(y_(i)−y_(c))²=r².Next, at block 88, a conventional method for calculating road curvatureusing yaw rate is utilized to identify an alternate road curvaturecalculation. This conventional curvature calculation using yaw rate isfor example, similar to the method disclosed in U.S. Pat. No. 5,926,126entitled “Method And System For Detecting An In-Path Target Obstacle InFront Of A Vehicle” and is incorporated herein by reference.Furthermore, a final road curvature is calculated by fusing (combining)the two calculations mv,x.z. Fusion of the yaw rate based road curvaturecalculation and target based road curvature calculation is achieved byfollowing the following rule as the change in the yaw rate increases theweight of the yaw-rate based curvature decreases, and as the number oftargets increases the weight of the target based curvature increases.

[0031] As any person skilled in the art of geometric based pathprediction methods will recognize from the previous detailed descriptionand from the figures and claims, modifications and changes can be madeto the preferred embodiments of the invention without departing from thescope of this invention defined in the following claims.

1. A method for estimating a radius of curvature in a road, the methodcomprising: measuring a range and an azimuth angle between a hostvehicle and a plurality of target vehicles; determining one of whetherthe host vehicle is changing lanes and whether a primary target vehiclein the plurality of target vehicles is changing lanes; calculating aheading angle of the host vehicle; calculating a corrected azimuth angleby adjusting the measured azimuth angle by the calculated heading angle;selecting a curve that minimizes a mean square error between theselected curve and a plurality of measured locations of the plurality oftarget vehicles; and determining an equation that describes the curve,wherein the equation is used to determine the radius of curvature of theroad ahead of the host vehicle.
 2. The method of claim 1 furthercomprising measuring a relative velocity between the host and targetvehicles.
 3. The method of claim 1 wherein the heading angle is definedas the angle between a line tangent to the road and a direction the hostvehicle is heading in.
 4. The method of claim 3 wherein calculating aheading angle (η) further comprises calculating the heading angle asdefined by: $\begin{matrix}{{{{\frac{V_{h}}{L}x\quad \alpha} + \alpha} = {{{- \eta}\quad {where}\text{:}V_{h}} = {{the}\quad {host}\quad {vechicle}\quad {speed}}}};} \\{{L = {{the}\quad {range}\quad {between}\quad {the}\quad {host}\quad {vehicle}\quad {and}\quad {the}\quad {target}\quad {vehicle}}};} \\{\alpha = {{the}\quad {azimuth}\quad {angle}\quad {between}\quad {the}\quad {host}\quad {vehicle}\quad {and}\quad {the}\quad {target}\quad {{vehicle}.}}}\end{matrix}$

L=the range between the host vehicle and the target vehicle; α=theazimuth angle between the host vehicle and the target vehicle.
 5. Themethod of claim 1 wherein measuring the azimuth angle further comprisesmeasuring the azimuth angle between a plurality of stopped and movingtargets.
 6. The method of claim 1 wherein determining whether one of ahost vehicle and a primary target vehicle is changing lanes furthercomprises determining whether the azimuth angle between the host vehicleand the plurality of target vehicles is changing in the same way.
 7. Themethod of claim 1 wherein calculating a corrected azimuth angle furthercomprises rotating the measured azimuth angle by the calculated headingangle.
 8. The method of claim 1 wherein selecting a curve furthercomprises selecting a circle having a predefined radius.
 9. A method forestimating a radius of curvature in a road, the method comprising:measuring a range and an azimuth angle between a host vehicle and aplurality of target vehicles and a plurality of stopped targets;determining one of whether the host vehicle is changing lanes andwhether a primary target vehicle in the plurality of target vehicles ischanging lanes; calculating a heading angle of the host vehicle;calculating a corrected azimuth angle by adjusting the measured azimuthangle by the calculated heading angle; selecting a curve that minimizesa mean square error between the selected curve and a plurality ofmeasured locations of the plurality of target vehicles and the pluralityof stopped targets; and determining an equation that describes thecurve, wherein the equation is used to predict the radius of curvatureof the road ahead of the host vehicle.
 10. The method of claim 9 whereinthe heading angle is defined as the angle between a line tangent to theroad and a direction the host vehicle is heading in.
 11. The method ofclaim 10 wherein calculating a heading angle (η) further comprisescalculating the heading angle as defined by: $\begin{matrix}{{{{\frac{V_{h}}{L}x\quad \alpha} + \alpha} = {{{- \eta}\quad {where}\text{:}V_{h}} = {{the}\quad {host}\quad {vechicle}\quad {speed}}}};} \\{{L = {{the}\quad {range}\quad {between}\quad {the}\quad {host}\quad {vehicle}\quad {and}\quad {the}\quad {target}\quad {vehicle}}};} \\{\alpha = {{the}\quad {azimuth}\quad {angle}\quad {between}\quad {the}\quad {host}\quad {vehicle}\quad {and}\quad {the}\quad {target}\quad {{vehicle}.}}}\end{matrix}$

L=the range between the host vehicle and the target vehicle; α=theazimuth angle between the host vehicle and the target vehicle.
 12. Themethod of claim 9 wherein determining whether one of a host vehicle anda primary target vehicle is changing lanes further comprises determiningwhether the azimuth angle between the host vehicle and the plurality oftarget vehicles is changing in the same way.
 13. The method of claim 9wherein calculating a corrected azimuth angle further comprises rotatingthe measured azimuth angle by the calculated heading angle.
 14. Themethod of claim 9 wherein selecting a curve further comprises selectinga circle having a predefined radius.